#ifndef REGISTRATION_H
#define REGISTRATION_H

#include "load.h"
#include "functions.h"
#include "keypoints.h"
#include "correspond.h"
#include "features.h"
#include "Alignment.h"
#include <math.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/angles.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/transformation_validation.h>
#include <pcl/registration/transformation_validation_euclidean.h>
#include <Eigen/StdVector>


using namespace std;
using namespace pcl;

struct regConfig
{
  int initialAlgorithm;
  float inputVoxelResolution;
  float initialScoreThreshold;
  float finalScoreThreshold;
  float initialResolution;
  float finalResolution;
  float clusterThreshold;
  double minRadiusFPFH;
  double maxRadiusFPFH;
  double stepRadiusFPFH;
  double minSACThresh;
  double maxSACThresh;
  double stepSACThresh;
  bool showViewer;
  float minClusterSize;
  float maxClusterSize;
  bool parallel;
  bool segmentPlane;
};

void registerAllWithAlignment(
        PointCloud<PointXYZ>::Ptr object ,
        PointCloud<PointXYZ>::Ptr world,
        vector<Alignment *> &alignmentAttempts,
        PointCloud<PointXYZI>::Ptr object_keypoints,
        regConfig config);




template< class PointT >//ANGLES
float
validationScoreProximityTargetAngles (PointCloud<PointT> &world, PointCloud<PointT> &object)
{
    float sum=0;
    boost::shared_ptr< PointCloud<PointT> > worldPtr= world.makeShared();
    search::KdTree<PointT> tree;
    PointCloud<Normal> worldNormals;
    PointCloud<Normal> objectNormals;
    onlyNormals(world,worldNormals,5);
    onlyNormals(object,objectNormals,5);
    tree.setInputCloud(worldPtr);
    vector<int> indices;
    vector<float> sqrDist;
    PointT currentPoint;
    for (int i=0;i<object.size();i++)
    {
        currentPoint=object.at(i);
        tree.nearestKSearch(currentPoint,1,indices,sqrDist);
        if(!isFinite(worldNormals.at(indices.at(0)))||!isFinite(objectNormals.at(i))) continue;
        float angle=getAngle3D(worldNormals.at(indices.at(0)).getNormalVector4fMap(),objectNormals.at(i).getNormalVector4fMap());
        sum+= pow(rad2deg(angle),2);
    }
    return sum/object.size();
}

template< class PointT >//PERCENT OF POINTS
float
validationScoreProximityTargetPercent (PointCloud<PointT> &world, PointCloud<PointT> &object)
{
    float distanceThreshold=4.0f;
    float sum=0;
    boost::shared_ptr< PointCloud<PointT> > worldPtr= world.makeShared();
    search::KdTree<PointT> tree;
    tree.setInputCloud(worldPtr);
    vector<int> indices;
    vector<float> sqrDist;
    PointT currentPoint;
    for (int i=0;i<object.size();i++)
    {
        currentPoint=object.at(i);
        tree.nearestKSearch(currentPoint,1,indices,sqrDist);
        if(sqrDist.at(0)>distanceThreshold) sum++;
    }
    return sum/object.size()*100;//Percentuale di punti che sono distanti almeno la soglia impostata
}


template< class PointT >//SQUARE DISTANCE
float
validationScoreProximityTargetEuclidean (PointCloud<PointT> &world, PointCloud<PointT> &object)
{
    float sum=0;
    boost::shared_ptr< PointCloud<PointT> > worldPtr= world.makeShared();
    search::KdTree<PointT> tree;
    tree.setInputCloud(worldPtr);
    vector<int> indices;
    vector<float> sqrDist;
    PointT currentPoint;
    for (int i=0;i<object.size();i++)
    {
        currentPoint=object.at(i);
        tree.nearestKSearch(currentPoint,1,indices,sqrDist);
        sum+=sqrDist.at(0);
    }
    return sum/object.size();
}





template< class PointT >
float
validationScoreProximityTargetApplyTransformation (PointCloud<PointT> world, PointCloud<PointT> object,Eigen::Matrix4f &transformMatrix)
{//source passata per valore: hard copia l'oggetto

    transformPointCloud(object,object,transformMatrix);
    //cutShapeAroundTarget(world,object);
    return validationScoreProximityTargetEuclidean (world, object);
}


#endif

